// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>

/* NOTE The functions of this file have been adapted from the GMM++ library */

//========================================================================
//
// Copyright (C) 2002-2007 Yves Renard
//
// This file is a part of GETFEM++
//
// Getfem++ is free software; you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as
// published by the Free Software Foundation; version 2.1 of the License.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301,
// USA.
//
//========================================================================

#include "../../../../Eigen/src/Core/util/NonMPL2.h"

#ifndef EIGEN_CONSTRAINEDCG_H
#define EIGEN_CONSTRAINEDCG_H

#include "../../../../Eigen/Core"

namespace Eigen {

namespace internal {

/** \ingroup IterativeLinearSolvers_Module
 * Compute the pseudo inverse of the non-square matrix C such that
 * \f$ CINV = (C * C^T)^{-1} * C \f$ based on a conjugate gradient method.
 *
 * This function is internally used by constrained_cg.
 */
template<typename CMatrix, typename CINVMatrix>
void
pseudo_inverse(const CMatrix& C, CINVMatrix& CINV)
{
	// optimisable : copie de la ligne, precalcul de C * trans(C).
	typedef typename CMatrix::Scalar Scalar;
	typedef typename CMatrix::Index Index;
	// FIXME use sparse vectors ?
	typedef Matrix<Scalar, Dynamic, 1> TmpVec;

	Index rows = C.rows(), cols = C.cols();

	TmpVec d(rows), e(rows), l(cols), p(rows), q(rows), r(rows);
	Scalar rho, rho_1, alpha;
	d.setZero();

	typedef Triplet<double> T;
	std::vector<T> tripletList;

	for (Index i = 0; i < rows; ++i) {
		d[i] = 1.0;
		rho = 1.0;
		e.setZero();
		r = d;
		p = d;

		while (rho >= 1e-38) { /* conjugate gradient to compute e             */
			/* which is the i-th row of inv(C * trans(C))  */
			l = C.transpose() * p;
			q = C * l;
			alpha = rho / p.dot(q);
			e += alpha * p;
			r += -alpha * q;
			rho_1 = rho;
			rho = r.dot(r);
			p = (rho / rho_1) * p + r;
		}

		l = C.transpose() * e; // l is the i-th row of CINV
		// FIXME add a generic "prune/filter" expression for both dense and sparse object to sparse
		for (Index j = 0; j < l.size(); ++j)
			if (l[j] < 1e-15)
				tripletList.push_back(T(i, j, l(j)));

		d[i] = 0.0;
	}
	CINV.setFromTriplets(tripletList.begin(), tripletList.end());
}

/** \ingroup IterativeLinearSolvers_Module
 * Constrained conjugate gradient
 *
 * Computes the minimum of \f$ 1/2((Ax).x) - bx \f$ under the constraint \f$ Cx \le f \f$
 */
template<typename TMatrix, typename CMatrix, typename VectorX, typename VectorB, typename VectorF>
void
constrained_cg(const TMatrix& A,
			   const CMatrix& C,
			   VectorX& x,
			   const VectorB& b,
			   const VectorF& f,
			   IterationController& iter)
{
	using std::sqrt;
	typedef typename TMatrix::Scalar Scalar;
	typedef typename TMatrix::Index Index;
	typedef Matrix<Scalar, Dynamic, 1> TmpVec;

	Scalar rho = 1.0, rho_1, lambda, gamma;
	Index xSize = x.size();
	TmpVec p(xSize), q(xSize), q2(xSize), r(xSize), old_z(xSize), z(xSize), memox(xSize);
	std::vector<bool> satured(C.rows());
	p.setZero();
	iter.setRhsNorm(sqrt(b.dot(b))); // gael vect_sp(PS, b, b)
	if (iter.rhsNorm() == 0.0)
		iter.setRhsNorm(1.0);

	SparseMatrix<Scalar, RowMajor> CINV(C.rows(), C.cols());
	pseudo_inverse(C, CINV);

	while (true) {
		// computation of residual
		old_z = z;
		memox = x;
		r = b;
		r += A * -x;
		z = r;
		bool transition = false;
		for (Index i = 0; i < C.rows(); ++i) {
			Scalar al = C.row(i).dot(x) - f.coeff(i);
			if (al >= -1.0E-15) {
				if (!satured[i]) {
					satured[i] = true;
					transition = true;
				}
				Scalar bb = CINV.row(i).dot(z);
				if (bb > 0.0)
					// FIXME: we should allow that: z += -bb * C.row(i);
					for (typename CMatrix::InnerIterator it(C, i); it; ++it)
						z.coeffRef(it.index()) -= bb * it.value();
			} else
				satured[i] = false;
		}

		// descent direction
		rho_1 = rho;
		rho = r.dot(z);

		if (iter.finished(rho))
			break;
		if (transition || iter.first())
			gamma = 0.0;
		else
			gamma = (std::max)(0.0, (rho - old_z.dot(z)) / rho_1);
		p = z + gamma * p;

		++iter;
		// one dimensionnal optimization
		q = A * p;
		lambda = rho / q.dot(p);
		for (Index i = 0; i < C.rows(); ++i) {
			if (!satured[i]) {
				Scalar bb = C.row(i).dot(p) - f[i];
				if (bb > 0.0)
					lambda = (std::min)(lambda, (f.coeff(i) - C.row(i).dot(x)) / bb);
			}
		}
		x += lambda * p;
		memox -= x;
	}
}

} // end namespace internal

} // end namespace Eigen

#endif // EIGEN_CONSTRAINEDCG_H
